from math import sqrt, sin, cos, pi

wgs84_a = 6378137.0
wgs84_f = 1 / 298.257223565
wgs84_e2 = wgs84_f * (2 - wgs84_f)
D2R = pi / 180.0


def uag(a):
	u = a
	while u > pi:
		u -= 2*pi
	while u <= -pi:
		u += 2*pi
	return u


class Position:
	init_latitude = 0
	init_longitude = 0
	init_altitude = 0

	def init(self, init_latitude, init_longitude, init_altitude):
		self.init_latitude = init_latitude
		self.init_longitude = init_longitude
		self.init_altitude = init_altitude

	def BLH2XYZ(self, latitude, longitude, altitude):
		lat0 = self.init_latitude * D2R
		lon0 = self.init_longitude * D2R
		alt0 = self.init_altitude
		N0 = wgs84_a / sqrt(1 - wgs84_e2 * sin(lat0) * sin(lat0))
		x0 = (N0 + alt0) * cos(lat0) * cos(lon0)
		y0 = (N0 + alt0) * cos(lat0) * sin(lon0)
		z0 = (N0 * (1 - wgs84_e2) + alt0) * sin(lat0)

		lat = latitude * D2R
		lon = longitude * D2R
		alt = altitude

		N = wgs84_a / sqrt(1 - wgs84_e2 * sin(lat) * sin(lat))
		x = (N + alt) * cos(lat) * cos(lon)
		y = (N + alt) * cos(lat) * sin(lon)
		z = (N * (1 - wgs84_e2) + alt) * sin(lat)

		dx = x - x0
		dy = y - y0
		dz = z - z0

		s11 = -sin(lon0)
		s12 = cos(lon0)
		s13 = 0
		s21 = -sin(lat0) * cos(lon0)
		s22 = -sin(lat0) * sin(lon0)
		s23 = cos(lat0)
		s31 = cos(lat0) * cos(lon0)
		s32 = cos(lat0) * sin(lon0)
		s33 = sin(lat0)

		egoX = 111000 * (lon - lon0) / D2R * cos(lat0)
		egoY = 111000 * (lat - lat0) / D2R
		egoZ = 0

		lstX = egoX
		lstY = egoY

		ex = egoX - lstX
		ey = egoY - lstY
		es = sqrt(ex * ex + ey * ey)
		if es > 100:
			return
		else:
			lstX = egoX
			lstY = egoY

		x = s11 * dx + s12 * dy + s13 * dz
		y = s21 * dx + s22 * dy + s23 * dz
		z = s31 * dx + s32 * dy + s33 * dz

		return [x, y, z]

	def FRD2FLU(self, rx, ry, rz):
		rx = uag(rx)
		ry = -uag(ry)
		rz = uag(rz)
		rrz = rz

		if rz >= 0:
			rrz = uag(pi/2 - rz)
		else:
			if rz >= -pi/2:
				rrz = uag(pi/2 - rz)
			else:
				rrz = uag(-3*pi/2 - rz)
		return [rx, ry, rrz]